#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include "wiringPi.h"
#include <softPwm.h>
#include "pthread.h"
#include "libServo.h"
#include "string.h"
#include "libServoShm.h"


//const char a[]="123";
int main(int argc, char *argv[]) {
    int re =0;
//    char *ab = a;

    re = Init_ServoShm();
    if(re==-1){
        printf("[ERROR] Init_ServoShm ERROR\n");
        return -1;
    }

    void *ServoShm_Pointer = Get_Shm_Pointer();
    if(ServoShm_Pointer==NULL){
        printf("[ERROR] Get_Shm_Pointer ERROR\n");
        return -1;
    }
    wiringPiSetup () ;
    re = motion_init();
    if (re==0){
        printf("motion_init success\n");
    }


    while(1){
        Set_Data_pram(ServoShm_Pointer,stepshm_X(1)+S_mode,1,1);
        Set_Data_pram(ServoShm_Pointer,stepshm_X(1)+S_circle,1,1);
        Set_Data_pram(ServoShm_Pointer,stepshm_X(1)+S_dir,0,1);
        Set_Data_pram(ServoShm_Pointer,stepshm_X(1)+S_speed,0,1);

       // Step_Cmd_Execute(1);
        while (Step_Cmd_Execute(1)==-1) { usleep(10);};
       // sleep(2);
        Set_Data_pram(ServoShm_Pointer,stepshm_X(1)+S_dir,0,1);
      //  Step_Cmd_Execute(1);
        //sleep(2);
        while (Step_Cmd_Execute(1)==-1) {usleep(10);};
    }



    return 0;
}
